/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christian Connette
 *********************************************************************/

#include <eband_local_planner/eband_trajectory_controller.h>
#include <tf2/utils.h>

namespace eband_local_planner
{

using std::min;
using std::max;

EBandTrajectoryCtrl::EBandTrajectoryCtrl() : costmap_ros_(NULL), initialized_(false), band_set_(false),
                                             visualization_(false) {}

EBandTrajectoryCtrl::EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
        : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false)
{
    // initialize planner
    initialize(name, costmap_ros);
    // Initialize pid object (note we'll be further clamping its output)
    pid_.initPid(1, 0, 0, 10, -10);
}

EBandTrajectoryCtrl::~EBandTrajectoryCtrl() {}

void EBandTrajectoryCtrl::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
    // check if trajectory controller is already initialized
    if (!initialized_)
    {
        // create Node Handle with name of plugin (as used in move_base for loading)
        ros::NodeHandle node_private("~/" + name);
        in_final_goal_turn_ = false;
        // copy adress of costmap and Transform Listener (handed over from move_base)
        costmap_ros_ = costmap_ros;
        // init velocity for interpolation
        last_vel_.linear.x = 0.0;
        last_vel_.linear.y = 0.0;
        last_vel_.linear.z = 0.0;
        last_vel_.angular.x = 0.0;
        last_vel_.angular.y = 0.0;
        last_vel_.angular.z = 0.0;
        // set the general refernce frame to that in which the band is given
        geometry_msgs::Pose2D tmp_pose2D;
        tmp_pose2D.x = 0.0;
        tmp_pose2D.y = 0.0;
        tmp_pose2D.theta = 0.0;
        Pose2DToPose(ref_frame_band_, tmp_pose2D);
        // set initialized flag
        initialized_ = true;
    }
    else ROS_WARN("This planner has already been initialized, doing nothing.");
}

void EBandTrajectoryCtrl::reconfigure(eband_local_planner::EBandPlannerConfig &config)
{
    max_vel_lin_ = config.max_vel_lin;
    max_vel_th_ = config.max_vel_th;
    min_vel_lin_ = config.min_vel_lin;
    min_vel_th_ = config.min_vel_th;
    min_in_place_vel_th_ = config.min_in_place_vel_th;
    in_place_trans_vel_ = config.in_place_trans_vel;
    tolerance_trans_ = config.xy_goal_tolerance;
    tolerance_rot_ = config.yaw_goal_tolerance;
    k_p_ = config.k_prop;
    k_nu_ = config.k_damp;
    ctrl_freq_ = config.Ctrl_Rate;
    acc_max_ = config.max_acceleration;
    virt_mass_ = config.virtual_mass;
    acc_max_trans_ = config.max_translational_acceleration;
    acc_max_rot_ = config.max_rotational_acceleration;
    rotation_correction_threshold_ = config.rotation_correction_threshold;
    // diffferential drive parameters
    differential_drive_hack_ = config.differential_drive;
    bubble_velocity_multiplier_ = config.bubble_velocity_multiplier;
    rotation_threshold_multiplier_ = config.rotation_threshold_multiplier;
    disallow_hysteresis_ = config.disallow_hysteresis;
}

void EBandTrajectoryCtrl::setVisualization(boost::shared_ptr<EBandVisualization> target_visual)
{
    target_visual_ = target_visual;
    visualization_ = true;
}

bool EBandTrajectoryCtrl::setBand(const std::vector<Bubble> &elastic_band)
{
    elastic_band_ = elastic_band;
    band_set_ = true;
    return true;
}

bool EBandTrajectoryCtrl::setOdometry(const nav_msgs::Odometry &odometry)
{
    odom_vel_.linear.x = odometry.twist.twist.linear.x;
    odom_vel_.linear.y = odometry.twist.twist.linear.y;
    odom_vel_.linear.z = 0.0;
    odom_vel_.angular.x = 0.0;
    odom_vel_.angular.y = 0.0;
    odom_vel_.angular.z = odometry.twist.twist.angular.z;
    return true;
}

// Return the angular difference between the direction we're pointing and the direction we want to move in
double angularDiff(const geometry_msgs::Twist &heading, const geometry_msgs::Pose &pose)
{
    const double pi = 3.14159265;
    const double t1 = atan2(heading.linear.y, heading.linear.x);
    const double t2 = tf2::getYaw(pose.orientation);
    const double d = t1 - t2;
    if (fabs(d) < pi) return d;
    else if (d < 0) return d + 2 * pi;
    else return d - 2 * pi;
}
// 差速速度控制
bool EBandTrajectoryCtrl::getTwistDifferentialDrive(geometry_msgs::Twist &twist_cmd, bool &goal_reached)
{
    goal_reached = false;
    geometry_msgs::Twist robot_cmd, bubble_diff;
    robot_cmd.linear.x = 0.0;
    robot_cmd.angular.z = 0.0;
    bool command_provided = false;
    // check if plugin initialized
    if (!initialized_)
    {
        ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner");
        return false;
    }
    // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan)
    // 路径太少则退出, 说明差不多到终点了或者路径规划错误
    if ((!band_set_) || (elastic_band_.size() < 2))
    {
        ROS_WARN("Requesting feedforward command from empty band.");
        return false;
    }
    // Get the differences between the first 2 bubbles in the robot's frame
    // 获取机器人当前位姿到规划路径上即将到达的下一个点的距离向量, 是个Twist
    bubble_diff = getFrame1ToFrame2InRefFrameNew(
            elastic_band_.at(0).center.pose,
            elastic_band_.at(1).center.pose,
            elastic_band_.at(0).center.pose);
    float distance_from_goal = -1.0f;
    // Check 1 快到终点的情况 We need to check if we are within the threshold of the final destination
    if (!command_provided)
    {
        int curr_target_bubble = 1;
        while (curr_target_bubble < ((int) elastic_band_.size()) - 1)
        {
            curr_target_bubble++;
            bubble_diff =
                    getFrame1ToFrame2InRefFrameNew(
                            elastic_band_.at(0).center.pose,
                            elastic_band_.at(curr_target_bubble).center.pose,
                            elastic_band_.at(0).center.pose);
        }
        // if you go past tolerance, then try to get closer again
        if (!disallow_hysteresis_)
        {
            if (fabs(bubble_diff.linear.x) > tolerance_trans_ ||
                fabs(bubble_diff.linear.y) > tolerance_trans_)
            {
                in_final_goal_turn_ = false;
            }
        }
        // Get the differences between the first 2 bubbles in the robot's frame
        int goal_bubble = (((int) elastic_band_.size()) - 1);
        // 计算当前位姿与局部地图下最远的目标点(截断全局路径的最后一个元素)的距离
        bubble_diff = getFrame1ToFrame2InRefFrameNew(
                elastic_band_.at(0).center.pose,
                elastic_band_.at(goal_bubble).center.pose,
                elastic_band_.at(0).center.pose);
        distance_from_goal = sqrtf(bubble_diff.linear.x * bubble_diff.linear.x +
                                   bubble_diff.linear.y * bubble_diff.linear.y);

        // Get closer to the goal than the tolerance requires before starting the final turn.
        // The final turn may cause you to move slightly out of position
        if ((fabs(bubble_diff.linear.x) <= 0.6 * tolerance_trans_ &&
             fabs(bubble_diff.linear.y) <= 0.6 * tolerance_trans_) ||
            in_final_goal_turn_)
        {
            // Calculate orientation difference to goal orientation (not captured in bubble_diff)
            double robot_yaw = tf2::getYaw(elastic_band_.at(0).center.pose.orientation);
            double goal_yaw = tf2::getYaw(elastic_band_.at((int) elastic_band_.size() - 1).center.pose.orientation);
            float orientation_diff = angles::normalize_angle(goal_yaw - robot_yaw);
            // 如果当前点与目标点方向角大于设置的旋转允差,那么原地旋转直到角度小于允差为止
            if (fabs(orientation_diff) > tolerance_rot_)
            {
                in_final_goal_turn_ = true;
                ROS_DEBUG("Performing in place rotation for goal (diff): %f", orientation_diff);
                double rotation_sign = -2 * (orientation_diff < 0) + 1;
                robot_cmd.angular.z = rotation_sign * min_in_place_vel_th_ + k_p_ * orientation_diff;
                // limit max rotation
                if (fabs(robot_cmd.angular.z) > max_vel_th_) robot_cmd.angular.z = rotation_sign * max_vel_th_;
            }
            else
            {
                in_final_goal_turn_ = false; // Goal reached
                ROS_INFO ("TrajectoryController: Goal reached with distance %.2f, %.2f (od = %.2f); "
                          "sending zero velocity", bubble_diff.linear.x, bubble_diff.linear.y, orientation_diff);
                // goal position reached
                robot_cmd.linear.x = 0.0;
                robot_cmd.angular.z = 0.0;
                goal_reached = true;
            }
            command_provided = true;
        }
    }
    // 2.未到终点的情况
    // Get the differences between the first 2 bubbles in the robot's frame
    bubble_diff = getFrame1ToFrame2InRefFrameNew(
            elastic_band_.at(0).center.pose,
            elastic_band_.at(1).center.pose,
            elastic_band_.at(0).center.pose);
    // Check 2 检查是否需要旋转 check if the robot's current pose is too misaligned with the next bubble
    if (!command_provided)
    {
        ROS_DEBUG("Goal has not been reached, performing checks to move towards goal");
        // calculate an estimate of the in-place rotation threshold
        // 到下一个泡泡的距离,即到规划路径上下一个点的距离
        double distance_to_next_bubble = sqrt(
                bubble_diff.linear.x * bubble_diff.linear.x +
                bubble_diff.linear.y * bubble_diff.linear.y);
        // 注意, 这里代码写死了 0.7 倍的泡泡半径, 有需要可以调调看看效果
        // 这个 expansion 在 eband_local_planner 中由 costmap 计算的到障碍物的距离转化得来
        double radius_of_next_bubble = 0.7 * elastic_band_.at(1).expansion;
        // 这里定义了一个原地旋转阈值 rotation_threshold_multiplier_ 作为参数在 yaml 文件中配置
        // 把这个参数调大可以增大旋转阈值,这样机器人更倾向于边走边转, 而不是原地旋转到正确方向再走
        // 调大这个参数就有一点儿全向移动那味儿了
        double in_place_rotation_threshold =
                rotation_threshold_multiplier_ *
                fabs(atan2(radius_of_next_bubble, distance_to_next_bubble));
        ROS_DEBUG("In-place rotation threshold: %f(%f,%f)",
                  in_place_rotation_threshold, radius_of_next_bubble, distance_to_next_bubble);
        // check if we are above this threshold, if so then perform in-place rotation
        // 当前泡泡(位姿)到下一个泡泡的角度差大于原地旋转阈值, 则原地旋转
        if (fabs(bubble_diff.angular.z) > in_place_rotation_threshold)
        {
            robot_cmd.angular.z = k_p_ * bubble_diff.angular.z;
            double rotation_sign = (bubble_diff.angular.z < 0) ? -1.0 : +1.0;
            if (fabs(robot_cmd.angular.z) < min_in_place_vel_th_)
            {
                robot_cmd.angular.z = rotation_sign * min_in_place_vel_th_;
            }
            // limit max rotation
            if (fabs(robot_cmd.angular.z) > max_vel_th_)
            {
                robot_cmd.angular.z = rotation_sign * max_vel_th_;
            }
            ROS_DEBUG("Performing in place rotation for start (diff): %f with rot vel: %f", bubble_diff.angular.z,
                      robot_cmd.angular.z);
            command_provided = true;
        }
    }
    // Check 3 - If we reach here, it means we need to use our PID controller to move towards the next bubble
    // 以下是用 pid 计算速度的操作
    if (!command_provided)
    {

        // Select a linear velocity (based on the current bubble radius)
        double forward_sign = -2 * (bubble_diff.linear.x < 0) + 1;
        double bubble_radius = 0.7 * elastic_band_.at(0).expansion;
        double velocity_multiplier = bubble_velocity_multiplier_ * bubble_radius;
        double max_vel_lin = max_vel_lin_;
        // 这里可以改成直接除2, 下面作者又会乘2, 所以不超过最大速度,
        // 但最小速度可以小于 0.15, max_vel_lin = max_vel_lin / 2;
        if (distance_from_goal < 0.75f)
        {
            max_vel_lin = (max_vel_lin < 0.3) ? 0.15 : max_vel_lin / 2;
        }
        // 差速控制比较简单, 线速度由于只有x方向, 直接置成最大线速度
        double linear_velocity = velocity_multiplier * max_vel_lin;
        linear_velocity *= cos(bubble_diff.angular.z); //decrease while turning
        if (fabs(linear_velocity) > max_vel_lin_)
        {
            linear_velocity = forward_sign * max_vel_lin_;
        }
        else if (fabs(linear_velocity) < min_vel_lin_)
        {
            linear_velocity = forward_sign * min_vel_lin_;
        }
        // Select an angular velocity (based on PID controller)
        double error = bubble_diff.angular.z;
        double rotation_sign = -2 * (bubble_diff.angular.z < 0) + 1;
        // 角速度由位姿差给出
        double angular_velocity = k_p_ * error;
        if (fabs(angular_velocity) > max_vel_th_)
        {
            angular_velocity = rotation_sign * max_vel_th_;
        }
        else if (fabs(angular_velocity) < min_vel_th_)
        {
            angular_velocity = rotation_sign * min_vel_th_;
        }
        ROS_DEBUG("Selected velocity: lin: %f, ang: %f", linear_velocity, angular_velocity);
        robot_cmd.linear.x = linear_velocity;
        robot_cmd.angular.z = angular_velocity;
        command_provided = true;
    }
    twist_cmd = robot_cmd;
    ROS_DEBUG("Final command: %f, %f", twist_cmd.linear.x, twist_cmd.angular.z);
    return true;
}
// 总的速度控制器
bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist &twist_cmd, bool &goal_reached)
{
    goal_reached = false;
    // 调用差速控制器
    if (differential_drive_hack_) return getTwistDifferentialDrive(twist_cmd, goal_reached);
    // 以下则为全向速度控制
    // init twist cmd to be handed back to caller
    geometry_msgs::Twist robot_cmd, bubble_diff, control_deviation;
    robot_cmd.linear.x = 0.0;
    robot_cmd.linear.y = 0.0;
    robot_cmd.linear.z = 0.0;
    robot_cmd.angular.x = 0.0;
    robot_cmd.angular.y = 0.0;
    robot_cmd.angular.z = 0.0;
    // make sure command vector is clean
    twist_cmd = robot_cmd;
    control_deviation = robot_cmd;
    // check if plugin initialized
    if (!initialized_)
    {
        ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner");
        return false;
    }
    // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan)
    if ((!band_set_) || (elastic_band_.size() < 2))
    {
        ROS_WARN("Requesting feedforward command from empty band.");
        return false;
    }
    // calc intersection of bubble-radius with sequence of vector connecting the bubbles
    // get distance to target from bubble-expansion
    double scaled_radius = 0.7 * elastic_band_.at(0).expansion;
    // get difference and distance between bubbles in odometry frame
    double bubble_distance, ang_pseudo_dist;
    bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, elastic_band_.at(1).center.pose, ref_frame_band_);
    // 这里会计算一个伪距离, diff_z*机器人外接圆半径(乘积是一段弧长)
    ang_pseudo_dist = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
    bubble_distance = sqrt(
            (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) +
            (ang_pseudo_dist * ang_pseudo_dist));
    if (visualization_)
    {
        target_visual_->publishBubble("ctrl_target", 1, target_visual_->blue, elastic_band_.at(0));
        target_visual_->publishBubble("ctrl_target", 2, target_visual_->blue, elastic_band_.at(1));
    }
    // by default our control deviation is the difference between the bubble centers
    double abs_ctrl_dev;
    control_deviation = bubble_diff;
    // diff_z*机器人外接圆半径(一段弧长)
    ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_);
    // 点到点距离+diff_z*机器人外接圆半径(一段弧长)
    abs_ctrl_dev = sqrt((control_deviation.linear.x * control_deviation.linear.x) +
                        (control_deviation.linear.y * control_deviation.linear.y) +
                        (ang_pseudo_dist * ang_pseudo_dist));
    // yet depending on the expansion of our bubble we might want to adapt this point
    // 如果放缩的泡泡半径 < 到下一个泡泡的距离; 说明相邻两个泡泡间隔较大,重叠部分较小
    if (scaled_radius < bubble_distance)
    {
        // triviale case - simply scale bubble_diff
        double scale_difference = scaled_radius / bubble_distance;
        // 两个泡泡间隔较大,重叠部分较小,说明路径两个点间距离较大; 故减小速度, 以应对突发情况
        bubble_diff.linear.x *= scale_difference;
        bubble_diff.linear.y *= scale_difference;
        bubble_diff.angular.z *= scale_difference;
        // set controls; control_deviation 这个很关键, 参与速度cmd计算
        control_deviation = bubble_diff;
    }
    // if scaled_radius = bubble_distance -- we have nothing to do at all
    // 如果放缩的泡泡半径 > 到下一个泡泡的距离; 说明相邻两个泡泡间隔较小,重叠部分较大
    if (scaled_radius > bubble_distance)
    {
        // o.k. now we have to do a little bit more -> check next but one bubble
        if (elastic_band_.size() > 2)
        {
            // get difference between next and next but one bubble
            double next_bubble_distance;
            // 计算当前点往后走两个泡泡的距离
            geometry_msgs::Twist next_bubble_diff;
            next_bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(1).center.pose,
                                                           elastic_band_.at(2).center.pose,
                                                           ref_frame_band_);
            ang_pseudo_dist = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
            next_bubble_distance = sqrt((next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
                                        (next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
                                        (ang_pseudo_dist * ang_pseudo_dist));
            // 如果放缩的泡泡半径 > 到下两个泡泡的距离; 说明相邻两个泡泡间隔较小,重叠部分很大
            // 即一个单位的放缩的泡泡半径可以覆盖到两个泡泡,泡泡间隔非常紧
            if (scaled_radius > (bubble_distance + next_bubble_distance))
            {
                // we should normally not end up here - but just to be sure
                // 适当增大速度,要不然太小了
                control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x;
                control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y;
                control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z;
                // done
                if (visualization_) target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2));
            }
            else
            {
                // 如果 到下一个泡泡的距离 < 放缩的泡泡半径 < 到下两个泡泡的距离
                if (visualization_) target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2));
                // we want to calculate intersection point of bubble ...
                // ... and vector connecting the following bubbles
                double b_distance, cosine_at_bub;
                double vec_prod, norm_vec1, norm_vec2;
                double ang_pseudo_dist1, ang_pseudo_dist2;
                // get distance between next bubble center and intersection point
                ang_pseudo_dist1 = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
                ang_pseudo_dist2 = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
                // careful! - we need this sign because of the direction of the vectors and the definition of the vector-product
                // 注意这里有个负号
                vec_prod = -((bubble_diff.linear.x * next_bubble_diff.linear.x) +
                             (bubble_diff.linear.y * next_bubble_diff.linear.y) +
                             (ang_pseudo_dist1 * ang_pseudo_dist2));
                // 到第一个泡泡中心的距离(加了一个伪弧长)
                norm_vec1 = sqrt((bubble_diff.linear.x * bubble_diff.linear.x) +
                                 (bubble_diff.linear.y * bubble_diff.linear.y) +
                                 (ang_pseudo_dist1 * ang_pseudo_dist1));
                // 第一个泡泡中心到第二个泡泡中心的距离(加了一个伪弧长)
                norm_vec2 = sqrt((next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
                                 (next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
                                 (ang_pseudo_dist2 * ang_pseudo_dist2));
                // reform the cosine-rule 这里用的某个规则重置 cos
                cosine_at_bub = vec_prod / norm_vec1 / norm_vec2;
                b_distance = bubble_distance * cosine_at_bub +
                        sqrt(scaled_radius * scaled_radius -
                             bubble_distance * bubble_distance * (1.0 - cosine_at_bub * cosine_at_bub));
                // get difference vector from next_bubble to intersection point
                // 这里计算了一个放缩因子
                double scale_next_difference = b_distance / next_bubble_distance;
                next_bubble_diff.linear.x *= scale_next_difference;
                next_bubble_diff.linear.y *= scale_next_difference;
                next_bubble_diff.angular.z *= scale_next_difference;
                // and finally get the control deviation
                control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x;
                control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y;
                control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z;
                // done
            }
        }
    }
    // plot control deviation
    ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_);
    abs_ctrl_dev = sqrt((control_deviation.linear.x * control_deviation.linear.x) +
                        (control_deviation.linear.y * control_deviation.linear.y) +
                        (ang_pseudo_dist * ang_pseudo_dist));
    if (visualization_)
    {
        // compose bubble from ctrl-target
        geometry_msgs::Pose2D tmp_bubble_2d, curr_bubble_2d;
        geometry_msgs::Pose tmp_pose;
        // init bubble for visualization
        Bubble new_bubble = elastic_band_.at(0);
        PoseToPose2D(elastic_band_.at(0).center.pose, curr_bubble_2d);
        tmp_bubble_2d.x = curr_bubble_2d.x + control_deviation.linear.x;
        tmp_bubble_2d.y = curr_bubble_2d.y + control_deviation.linear.y;
        tmp_bubble_2d.theta = curr_bubble_2d.theta + control_deviation.angular.z;
        Pose2DToPose(tmp_pose, tmp_bubble_2d);
        new_bubble.center.pose = tmp_pose;
        new_bubble.expansion = 0.1; // just draw a small bubble
        target_visual_->publishBubble("ctrl_target", 0, target_visual_->red, new_bubble);
    }
    // 这里计算当前位姿到终点还有多远
    const geometry_msgs::Point &goal = (--elastic_band_.end())->center.pose.position;
    const double dx = elastic_band_.at(0).center.pose.position.x - goal.x;
    const double dy = elastic_band_.at(0).center.pose.position.y - goal.y;
    const double dist_to_goal = sqrt(dx * dx + dy * dy);
    // Assuming we're far enough from the final goal, make sure to rotate so we're facing the right way
    // 如果到最终目标点距离大于阈值,出发前要校正方向
    // 即使是全向轮,也要校正方向; 调大这个阈值, 只要在阈值范围内, 规划出来的路径可以全向移动!
    if (dist_to_goal > rotation_correction_threshold_)
    {
        const double angular_diff = angularDiff(control_deviation, elastic_band_.at(0).center.pose);
        const double vel = pid_.computeCommand(angular_diff, ros::Duration(1 / ctrl_freq_));
        const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_ / fabs(vel) : 1.0;
        control_deviation.angular.z = vel * mult;
        const double abs_vel = fabs(control_deviation.angular.z);
        ROS_DEBUG_THROTTLE_NAMED (
                1.0, "angle_correction",
                "Angular diff is %.2f and desired angular vel is %.2f.  Initial translation velocity is %.2f, %.2f",
                angular_diff, control_deviation.angular.z, control_deviation.linear.x, control_deviation.linear.y);
        // There are some weird tf errors if I let it be 0
        const double trans_mult = max(0.01, 1.0 - abs_vel / max_vel_th_);
        control_deviation.linear.x *= trans_mult;
        control_deviation.linear.y *= trans_mult;
        ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction",
                                  "Translation multiplier is %.2f and scaled translational velocity is %.2f, %.2f",
                                  trans_mult, control_deviation.linear.x, control_deviation.linear.y);
    }
    else ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction",
                                  "Not applying angle correction because distance to goal is %.2f", dist_to_goal);
    // 以下主要是通过 Pid 控制速度的 scale 以及连续性
    // now the actual control procedure start (using attractive Potentials)
    geometry_msgs::Twist desired_velocity, currbub_maxvel_dir;
    double desvel_abs, desvel_abs_trans, currbub_maxvel_abs;
    double scale_des_vel;
    desired_velocity = robot_cmd;
    currbub_maxvel_dir = robot_cmd;
    // calculate "equilibrium velocity" (Khatib86 - Realtime Obstacle Avoidance)
    // k_p_, k_nu_ 分别是参数文件里的 k_prop 和 k_damp
    desired_velocity.linear.x = k_p_ / k_nu_ * control_deviation.linear.x;
    desired_velocity.linear.y = k_p_ / k_nu_ * control_deviation.linear.y;
    desired_velocity.angular.z = k_p_ / k_nu_ * control_deviation.angular.z;
    //robot_cmd = desired_velocity;
    // get max vel for current bubble
    int curr_bub_num = 0;
    currbub_maxvel_abs = getBubbleTargetVel(curr_bub_num, elastic_band_, currbub_maxvel_dir);
    // if neccessarry scale desired vel to stay lower than currbub_maxvel_abs
    ang_pseudo_dist = desired_velocity.angular.z * getCircumscribedRadius(*costmap_ros_);
    desvel_abs = sqrt((desired_velocity.linear.x * desired_velocity.linear.x) +
                      (desired_velocity.linear.y * desired_velocity.linear.y) +
                      (ang_pseudo_dist * ang_pseudo_dist));
    if (desvel_abs > currbub_maxvel_abs)
    {
        scale_des_vel = currbub_maxvel_abs / desvel_abs;
        desired_velocity.linear.x *= scale_des_vel;
        desired_velocity.linear.y *= scale_des_vel;
        desired_velocity.angular.z *= scale_des_vel;
    }
    // make sure to stay within velocity bounds for the robot
    desvel_abs_trans = sqrt((desired_velocity.linear.x * desired_velocity.linear.x) +
                            (desired_velocity.linear.y * desired_velocity.linear.y));
    // for translation
    if (desvel_abs_trans > max_vel_lin_)
    {
        scale_des_vel = max_vel_lin_ / desvel_abs_trans;
        desired_velocity.linear.x *= scale_des_vel;
        desired_velocity.linear.y *= scale_des_vel;
        // to make sure we are staying inside the bubble also scale rotation
        desired_velocity.angular.z *= scale_des_vel;
    }
    // for rotation
    if (fabs(desired_velocity.angular.z) > max_vel_th_)
    {
        scale_des_vel = max_vel_th_ / fabs(desired_velocity.angular.z);
        desired_velocity.angular.z *= scale_des_vel;
        // to make sure we are staying inside the bubble also scale translation
        desired_velocity.linear.x *= scale_des_vel;
        desired_velocity.linear.y *= scale_des_vel;
    }
    // calculate resulting force (accel. resp.) (Khatib86 - Realtime Obstacle Avoidance)
    geometry_msgs::Twist acc_desired;
    acc_desired = robot_cmd;
    acc_desired.linear.x = (1.0 / virt_mass_) * k_nu_ * (desired_velocity.linear.x - last_vel_.linear.x);
    acc_desired.linear.y = (1.0 / virt_mass_) * k_nu_ * (desired_velocity.linear.y - last_vel_.linear.y);
    acc_desired.angular.z = (1.0 / virt_mass_) * k_nu_ * (desired_velocity.angular.z - last_vel_.angular.z);
    // constrain acceleration
    double scale_acc;
    double abs_acc_trans = sqrt(
            (acc_desired.linear.x * acc_desired.linear.x) + (acc_desired.linear.y * acc_desired.linear.y));
    if (abs_acc_trans > acc_max_trans_)
    {
        scale_acc = acc_max_trans_ / abs_acc_trans;
        acc_desired.linear.x *= scale_acc;
        acc_desired.linear.y *= scale_acc;
        // again - keep relations - stay in bubble
        acc_desired.angular.z *= scale_acc;
    }
    if (fabs(acc_desired.angular.z) > acc_max_rot_)
    {
        scale_acc = fabs(acc_desired.angular.z) / acc_max_rot_;
        acc_desired.angular.z *= scale_acc;
        // again - keep relations - stay in bubble
        acc_desired.linear.x *= scale_acc;
        acc_desired.linear.y *= scale_acc;
    }
    // and get velocity-cmds by integrating them over one time-step
    last_vel_.linear.x = last_vel_.linear.x + acc_desired.linear.x / ctrl_freq_;
    last_vel_.linear.y = last_vel_.linear.y + acc_desired.linear.y / ctrl_freq_;
    last_vel_.angular.z = last_vel_.angular.z + acc_desired.angular.z / ctrl_freq_;
    // we are almost done now take into accoun stick-slip and similar nasty things
    // last checks - limit current twist cmd (upper and lower bounds)
    last_vel_ = limitTwist(last_vel_);
    // finally set robot_cmd (to non-zero value)
    robot_cmd = last_vel_;
    // now convert into robot-body frame
    robot_cmd = transformTwistFromFrame1ToFrame2(robot_cmd, ref_frame_band_, elastic_band_.at(0).center.pose);
    // check whether we reached the end of the band
    int curr_target_bubble = 1;
    while (fabs(bubble_diff.linear.x) <= tolerance_trans_ &&
           fabs(bubble_diff.linear.y) <= tolerance_trans_ &&
           fabs(bubble_diff.angular.z) <= tolerance_rot_)
    {
        if (curr_target_bubble < ((int) elastic_band_.size()) - 1)
        {
            curr_target_bubble++;
            // transform next target bubble into robot-body frame
            // and get difference to robot bubble
            bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose,
                                                      elastic_band_.at(curr_target_bubble).center.pose, ref_frame_band_);
        }
        else
        {
            ROS_DEBUG_THROTTLE_NAMED (1.0, "controller_state",
                                      "Goal reached with distance %.2f, %.2f, %.2f ; sending zero velocity",
                                      bubble_diff.linear.x, bubble_diff.linear.y, bubble_diff.angular.z);
            // goal position reached
            robot_cmd.linear.x = 0.0;
            robot_cmd.linear.y = 0.0;
            robot_cmd.angular.z = 0.0;
            // reset velocity
            last_vel_.linear.x = 0.0;
            last_vel_.linear.y = 0.0;
            last_vel_.angular.z = 0.0;
            goal_reached = true;
            break;
        }
    }
    twist_cmd = robot_cmd;
    return true;
}

double EBandTrajectoryCtrl::getBubbleTargetVel(const int &target_bub_num, const std::vector<Bubble> &band,
                                               geometry_msgs::Twist &VelDir)
{
    // init reference for direction vector
    VelDir.linear.x = 0.0;
    VelDir.linear.y = 0.0;
    VelDir.linear.z = 0.0;
    VelDir.angular.x = 0.0;
    VelDir.angular.y = 0.0;
    VelDir.angular.z = 0.0;
    // if we are looking at the last bubble - target vel is always zero
    if (target_bub_num >= ((int) band.size() - 1)) return 0.0;
    // otherwise check for max_vel calculated from current bubble size
    double v_max_curr_bub, v_max_next_bub;
    double bubble_distance, angle_to_pseudo_vel, delta_vel_max;
    geometry_msgs::Twist bubble_diff;
    // distance for braking s = 0.5*v*v/a
    v_max_curr_bub = sqrt(2 * elastic_band_.at(target_bub_num).expansion * acc_max_);
    // get distance to next bubble center
    ROS_ASSERT((target_bub_num >= 0) && ((target_bub_num + 1) < (int) band.size()));
    bubble_diff = getFrame1ToFrame2InRefFrame(band.at(target_bub_num).center.pose,
                                              band.at(target_bub_num + 1).center.pose, ref_frame_band_);
    angle_to_pseudo_vel = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
    bubble_distance = sqrt(
            (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) +
            (angle_to_pseudo_vel * angle_to_pseudo_vel));
    // calculate direction vector - norm of diference
    VelDir.linear.x = bubble_diff.linear.x / bubble_distance;
    VelDir.linear.y = bubble_diff.linear.y / bubble_distance;
    VelDir.angular.z = bubble_diff.angular.z / bubble_distance;
    // if next bubble outside this one we will always be able to break fast enough
    if (bubble_distance > band.at(target_bub_num).expansion) return v_max_curr_bub;
    // next bubble center inside this bubble - take into account restrictions on next bubble
    int next_bub_num = target_bub_num + 1;
    geometry_msgs::Twist dummy_twist;
    v_max_next_bub = getBubbleTargetVel(next_bub_num, band, dummy_twist); // recursive call
    // if velocity at next bubble bigger (or equal) than our velocity - we are on the safe side
    if (v_max_next_bub >= v_max_curr_bub) return v_max_curr_bub;
    // otherwise max. allowed vel is next vel + plus possible reduction on the way between the bubble-centers
    delta_vel_max = sqrt(2 * bubble_distance * acc_max_);
    v_max_curr_bub = v_max_next_bub + delta_vel_max;
    return v_max_curr_bub;
}

geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose &frame1,
                                                                      const geometry_msgs::Pose &frame2,
                                                                      const geometry_msgs::Pose &ref_frame)
{
    geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D, ref_frame_pose2D;
    geometry_msgs::Pose2D frame1_pose2D_rf, frame2_pose2D_rf;
    geometry_msgs::Twist frame_diff;
    // transform all frames to Pose2d
    PoseToPose2D(frame1, frame1_pose2D);
    PoseToPose2D(frame2, frame2_pose2D);
    PoseToPose2D(ref_frame, ref_frame_pose2D);
    // transform frame1 into ref frame
    frame1_pose2D_rf.x = (frame1_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) +
                         (frame1_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta);
    frame1_pose2D_rf.y = -(frame1_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) +
                         (frame1_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta);
    frame1_pose2D_rf.theta = frame1_pose2D.theta - ref_frame_pose2D.theta;
    frame1_pose2D_rf.theta = angles::normalize_angle(frame1_pose2D_rf.theta);
    // transform frame2 into ref frame
    frame2_pose2D_rf.x = (frame2_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) +
                         (frame2_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta);
    frame2_pose2D_rf.y = -(frame2_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) +
                         (frame2_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta);
    frame2_pose2D_rf.theta = frame2_pose2D.theta - ref_frame_pose2D.theta;
    frame2_pose2D_rf.theta = angles::normalize_angle(frame2_pose2D_rf.theta);
    // get differences
    frame_diff.linear.x = frame2_pose2D_rf.x - frame1_pose2D_rf.x;
    frame_diff.linear.y = frame2_pose2D_rf.y - frame1_pose2D_rf.y;
    frame_diff.linear.z = 0.0;
    frame_diff.angular.x = 0.0;
    frame_diff.angular.y = 0.0;
    frame_diff.angular.z = frame2_pose2D_rf.theta - frame1_pose2D_rf.theta;
    // normalize angle
    frame_diff.angular.z = angles::normalize_angle(frame_diff.angular.z);
    return frame_diff;
}

geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose &frame1,
                                                                         const geometry_msgs::Pose &frame2,
                                                                         const geometry_msgs::Pose &ref_frame)
{
    double x1 = frame1.position.x - ref_frame.position.x;
    double y1 = frame1.position.y - ref_frame.position.y;
    double x2 = frame2.position.x - ref_frame.position.x;
    double y2 = frame2.position.y - ref_frame.position.y;
    double yaw_ref = tf2::getYaw(ref_frame.orientation);
    double x_diff = x2 - x1;
    double y_diff = y2 - y1;
    double theta_diff = atan2(y_diff, x_diff);
    // Now project this vector on to the reference frame
    double rotation = angles::normalize_angle(yaw_ref);
    double x_final = x_diff * cos(rotation) + y_diff * sin(rotation);
    double y_final = -x_diff * sin(rotation) + y_diff * cos(rotation);
    geometry_msgs::Twist twist_msg;
    twist_msg.linear.x = x_final;
    twist_msg.linear.y = y_final;
    twist_msg.angular.z = angles::normalize_angle(theta_diff - yaw_ref);
    return twist_msg;
}

geometry_msgs::Twist EBandTrajectoryCtrl::transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist &curr_twist,
                                                                           const geometry_msgs::Pose &frame1,
                                                                           const geometry_msgs::Pose &frame2)
{
    geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D;
    geometry_msgs::Twist tmp_transformed;
    double delta_ang;
    tmp_transformed = curr_twist;
    // transform all frames to Pose2d
    PoseToPose2D(frame1, frame1_pose2D);
    PoseToPose2D(frame2, frame2_pose2D);
    // get orientation diff of frames
    delta_ang = frame2_pose2D.theta - frame1_pose2D.theta;
    delta_ang = angles::normalize_angle(delta_ang);
    // tranform twist
    tmp_transformed.linear.x = curr_twist.linear.x * cos(delta_ang) + curr_twist.linear.y * sin(delta_ang);
    tmp_transformed.linear.y = -curr_twist.linear.x * sin(delta_ang) + curr_twist.linear.y * cos(delta_ang);
    return tmp_transformed;
}

geometry_msgs::Twist EBandTrajectoryCtrl::limitTwist(const geometry_msgs::Twist &twist)
{
    geometry_msgs::Twist res = twist;
    //make sure to bound things by our velocity limits
    double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
    double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y);
    if (lin_overshoot > 1.0)
    {
        res.linear.x /= lin_overshoot;
        res.linear.y /= lin_overshoot;
        // keep relations
        res.angular.z /= lin_overshoot;
    }
    //we only want to enforce a minimum velocity if we're not rotating in place
    if (lin_undershoot > 1.0)
    {
        res.linear.x *= lin_undershoot;
        res.linear.y *= lin_undershoot;
        // we cannot keep relations here for stability reasons
    }
    if (fabs(res.angular.z) > max_vel_th_)
    {
        double scale = max_vel_th_ / fabs(res.angular.z);
        //res.angular.z = max_vel_th_ * sign(res.angular.z);
        res.angular.z *= scale;
        // keep relations
        res.linear.x *= scale;
        res.linear.y *= scale;
    }
    if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
    // we cannot keep relations here for stability reasons
    //we want to check for whether or not we're desired to rotate in place
    if (sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_)
    {
        if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
        res.linear.x = 0.0;
        res.linear.y = 0.0;
    }
    ROS_DEBUG("Angular command %f", res.angular.z);
    return res;
}

}
